#include "State_JumpR2.h"
#include "Dog.h"

#define POINTANGLE1 RAD(100)
#define KP1 12.0f
#define KD1 0.4f

enum jump_state
{
    READY_STATE,
    JUMPING_STATE,
    SWING_STATE,
    FALLING_STATE,
} jumpState;

void JumpR2_Enter(void)
{
    UART_Print("JumpR2 enter\n");

    /* 蹬腿跳跃 */
    HT_SetPosition(dog.legs[0].motorF, ANGLE_F + global.jumpAngleR+POINTANGLE1, KP1, KD1, 0);
    HT_SetPosition(dog.legs[1].motorF, -ANGLE_F+global.jumpAngleL-POINTANGLE1, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorF, -ANGLE_F+global.jumpAngleL-POINTANGLE1, KP1, KD1, 0);
    HT_SetPosition(dog.legs[3].motorF, ANGLE_F + global.jumpAngleR+POINTANGLE1, KP1, KD1, 0);

    HT_SetPosition(dog.legs[0].motorB, -ANGLE_B - global.jumpAngleR+POINTANGLE1, KP1, KD1, 0);
    HT_SetPosition(dog.legs[1].motorB, ANGLE_B-global.jumpAngleL-POINTANGLE1, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorB, ANGLE_B-global.jumpAngleL-POINTANGLE1, KP1, KD1, 0);
    HT_SetPosition(dog.legs[3].motorB, -ANGLE_B - global.jumpAngleR+POINTANGLE1, KP1, KD1, 0);


    jumpState = JUMPING_STATE; // 跳跃状态中
}

#define KP2 4.0f
#define KD2 0.3f
#define KP3 8.0f
#define KD3 3.2f

void JumpR2_Run(void)
{
    // UART_Print("Jump2 run\n");

    switch (jumpState)
    {
    case JUMPING_STATE:
        if (HT_ArrivalPos(dog.legs[0].motorF, 0.2) &&
            HT_ArrivalPos(dog.legs[1].motorF, 0.2) &&
            HT_ArrivalPos(dog.legs[2].motorF, 0.2) &&
            HT_ArrivalPos(dog.legs[3].motorF, 0.2))
        {
            HT_SetPosition(dog.legs[0].motorF, ANGLE_F + global.jumpAngleR, KP2, KD2, 0);
            HT_SetPosition(dog.legs[1].motorF, -ANGLE_F+global.jumpAngleL, KP2, KD2, 0);
            HT_SetPosition(dog.legs[2].motorF, -ANGLE_F+global.jumpAngleL, KP2, KD2, 0);
            HT_SetPosition(dog.legs[3].motorF, ANGLE_F + global.jumpAngleR, KP2, KD2, 0);

            HT_SetPosition(dog.legs[0].motorB, -ANGLE_B - global.jumpAngleR, KP2, KD2, 0);
            HT_SetPosition(dog.legs[1].motorB, ANGLE_B-global.jumpAngleL, KP2, KD2, 0);
            HT_SetPosition(dog.legs[2].motorB, ANGLE_B-global.jumpAngleL, KP2, KD2, 0);
            HT_SetPosition(dog.legs[3].motorB, -ANGLE_B - global.jumpAngleR, KP2, KD2, 0);
            jumpState = SWING_STATE; // 摆动状态中
        }
        break;

    case SWING_STATE:
        if (HT_ArrivalPos(dog.legs[0].motorF, 0.2) &&
            HT_ArrivalPos(dog.legs[1].motorF, 0.2) &&
            HT_ArrivalPos(dog.legs[2].motorF, 0.2) &&
            HT_ArrivalPos(dog.legs[3].motorF, 0.2))
        {
            
        HT_SetPosition(dog.legs[0].motorF, ANGLE_F + global.jumpAngleR, KP3, KD3, 0);
        HT_SetPosition(dog.legs[1].motorF, -ANGLE_F+global.jumpAngleL, KP3, KD3, 0);
        HT_SetPosition(dog.legs[2].motorF, -ANGLE_F+global.jumpAngleL, KP3, KD3, 0);
        HT_SetPosition(dog.legs[3].motorF, ANGLE_F + global.jumpAngleR, KP3, KD3, 0);

        HT_SetPosition(dog.legs[0].motorB, -ANGLE_B - global.jumpAngleR, KP3, KD3, 0);
        HT_SetPosition(dog.legs[1].motorB, ANGLE_B-global.jumpAngleL, KP3, KD3, 0);
        HT_SetPosition(dog.legs[2].motorB, ANGLE_B-global.jumpAngleL, KP3, KD3, 0);
        HT_SetPosition(dog.legs[3].motorB, -ANGLE_B - global.jumpAngleR, KP3, KD3, 0);

            jumpState = FALLING_STATE; // 落地状态中
        }
        break;

    default:
        break;
    }
}

void JumpR2_Exit(void)
{
    UART_Print("JumpR2 exit\n");
}